<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>blmc_robots: /workspace/software/workspace/src/catkin/robots/blmc_robots/include/blmc_robots/n_joint_blmc_robot_driver.hpp Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">blmc_robots
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
      <li><a href="globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('n__joint__blmc__robot__driver_8hpp_source.html','');});
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">n_joint_blmc_robot_driver.hpp</div>  </div>
</div><!--header-->
<div class="contents">
<a href="n__joint__blmc__robot__driver_8hpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#pragma once</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;</div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="preprocessor">#include &lt;cmath&gt;</span></div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;</div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;<span class="preprocessor">#include &lt;Eigen/Eigen&gt;</span></div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;</div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;<span class="preprocessor">#include &lt;mpi_cpp_tools/math.hpp&gt;</span></div><div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="preprocessor">#include &lt;robot_interfaces/n_joint_robot_types.hpp&gt;</span></div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="preprocessor">#include &lt;robot_interfaces/robot_driver.hpp&gt;</span></div><div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;</div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;<span class="preprocessor">#include &lt;<a class="code" href="blmc__joint__module_8hpp.html">blmc_robots/blmc_joint_module.hpp</a>&gt;</span></div><div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;<span class="preprocessor">#include &lt;<a class="code" href="common__header_8hpp.html">blmc_robots/common_header.hpp</a>&gt;</span></div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;</div><div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;<span class="keyword">namespace </span><a class="code" href="namespaceblmc__robots.html">blmc_robots</a></div><div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;{</div><div class="line"><a name="l00026"></a><span class="lineno"><a class="line" href="structblmc__robots_1_1MotorParameters.html">   26</a></span>&#160;<span class="keyword">struct </span><a class="code" href="structblmc__robots_1_1MotorParameters.html">MotorParameters</a></div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;{</div><div class="line"><a name="l00029"></a><span class="lineno"><a class="line" href="structblmc__robots_1_1MotorParameters.html#ae8870c744aae11567280006ea7b7ed25">   29</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="structblmc__robots_1_1MotorParameters.html#ae8870c744aae11567280006ea7b7ed25">max_current_A</a>;</div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;</div><div class="line"><a name="l00032"></a><span class="lineno"><a class="line" href="structblmc__robots_1_1MotorParameters.html#a960e7f64dae8cd08837f8bff620eacfb">   32</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="structblmc__robots_1_1MotorParameters.html#a960e7f64dae8cd08837f8bff620eacfb">torque_constant_NmpA</a>;</div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;</div><div class="line"><a name="l00040"></a><span class="lineno"><a class="line" href="structblmc__robots_1_1MotorParameters.html#a6a8158b9ac1633bab6181043d04ab655">   40</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="structblmc__robots_1_1MotorParameters.html#a6a8158b9ac1633bab6181043d04ab655">gear_ratio</a>;</div><div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;};</div><div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;</div><div class="line"><a name="l00046"></a><span class="lineno"><a class="line" href="structblmc__robots_1_1CalibrationParameters.html">   46</a></span>&#160;<span class="keyword">struct </span><a class="code" href="structblmc__robots_1_1CalibrationParameters.html">CalibrationParameters</a></div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;{</div><div class="line"><a name="l00049"></a><span class="lineno"><a class="line" href="structblmc__robots_1_1CalibrationParameters.html#a85b403cbe9a0720b3b6ed46187e31fa6">   49</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="structblmc__robots_1_1CalibrationParameters.html#a85b403cbe9a0720b3b6ed46187e31fa6">torque_ratio</a>;  <span class="comment">// TODO better used fixed torque</span></div><div class="line"><a name="l00051"></a><span class="lineno"><a class="line" href="structblmc__robots_1_1CalibrationParameters.html#a84662e07c2b77e035e28092584ea0ab3">   51</a></span>&#160;<span class="comment"></span>    <span class="keywordtype">double</span> <a class="code" href="structblmc__robots_1_1CalibrationParameters.html#a84662e07c2b77e035e28092584ea0ab3">position_tolerance_rad</a>;</div><div class="line"><a name="l00053"></a><span class="lineno"><a class="line" href="structblmc__robots_1_1CalibrationParameters.html#a1075d6b60013e060c22748a672bf08cb">   53</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="structblmc__robots_1_1CalibrationParameters.html#a1075d6b60013e060c22748a672bf08cb">move_timeout</a>;</div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;};</div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;</div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;<span class="keyword">template</span> &lt;<span class="keywordtype">size_t</span> N_JOINTS, <span class="keywordtype">size_t</span> N_MOTOR_BOARDS&gt;</div><div class="line"><a name="l00066"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html">   66</a></span>&#160;<span class="keyword">class </span><a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html">NJointBlmcRobotDriver</a></div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;    : <span class="keyword">public</span> robot_interfaces::RobotDriver&lt;</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;          typename robot_interfaces::NJointRobotTypes&lt;N_JOINTS&gt;::Action,</div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;          typename robot_interfaces::NJointRobotTypes&lt;N_JOINTS&gt;::Observation&gt;</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;{</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;<span class="keyword">public</span>:</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;    <span class="keyword">typedef</span> <span class="keyword">typename</span> robot_interfaces::NJointRobotTypes&lt;N_JOINTS&gt; Types;</div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;    <span class="keyword">typedef</span> <span class="keyword">typename</span> Types::Action Action;</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;    <span class="keyword">typedef</span> <span class="keyword">typename</span> Types::Observation Observation;</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;    <span class="keyword">typedef</span> <span class="keyword">typename</span> Types::Vector Vector;</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;    <span class="keyword">typedef</span> std::array&lt;std::shared_ptr&lt;blmc_drivers::MotorInterface&gt;, N_JOINTS&gt;</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;        Motors;</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;    <span class="keyword">typedef</span> std::array&lt;std::shared_ptr&lt;blmc_drivers::CanBusMotorBoard&gt;,</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;                       N_MOTOR_BOARDS&gt;</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;        MotorBoards;</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;</div><div class="line"><a name="l00095"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a4da9d51841e9b2995e597ae52c460586">   95</a></span>&#160;    <span class="keyword">const</span> <span class="keywordtype">bool</span> <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a4da9d51841e9b2995e597ae52c460586">has_endstop_</a>;</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;    <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html">NJointBlmcRobotDriver</a>(<span class="keyword">const</span> MotorBoards &amp;motor_boards,</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;                          <span class="keyword">const</span> Motors &amp;motors,</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;                          <span class="keyword">const</span> <a class="code" href="structblmc__robots_1_1MotorParameters.html">MotorParameters</a> &amp;motor_parameters,</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;                          <span class="keyword">const</span> <a class="code" href="structblmc__robots_1_1CalibrationParameters.html">CalibrationParameters</a> &amp;calibration_parameters,</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;                          <span class="keyword">const</span> Vector &amp;safety_kd,</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;                          <span class="keyword">const</span> Vector &amp;position_kp,</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;                          <span class="keyword">const</span> Vector &amp;position_kd,</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;                          <span class="keyword">const</span> <span class="keywordtype">bool</span> has_endstop)</div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;        : <a class="code" href="namespacerobot__interfaces.html">robot_interfaces</a>::RobotDriver&lt;Action, Observation&gt;(),</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;          has_endstop_(has_endstop),</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;          joint_modules_(motors,</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;                         motor_parameters.<a class="code" href="structblmc__robots_1_1MotorParameters.html#a960e7f64dae8cd08837f8bff620eacfb">torque_constant_NmpA</a> * Vector::Ones(),</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;                         motor_parameters.<a class="code" href="structblmc__robots_1_1MotorParameters.html#a6a8158b9ac1633bab6181043d04ab655">gear_ratio</a> * Vector::Ones(),</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;                         Vector::Zero(),</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;                         motor_parameters.<a class="code" href="structblmc__robots_1_1MotorParameters.html#ae8870c744aae11567280006ea7b7ed25">max_current_A</a> * Vector::Ones()),</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;          motor_boards_(motor_boards),</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;          max_torque_Nm_(motor_parameters.<a class="code" href="structblmc__robots_1_1MotorParameters.html#ae8870c744aae11567280006ea7b7ed25">max_current_A</a> *</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;                         motor_parameters.<a class="code" href="structblmc__robots_1_1MotorParameters.html#a960e7f64dae8cd08837f8bff620eacfb">torque_constant_NmpA</a> *</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;                         motor_parameters.<a class="code" href="structblmc__robots_1_1MotorParameters.html#a6a8158b9ac1633bab6181043d04ab655">gear_ratio</a>),</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;          calibration_parameters_(calibration_parameters),</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;          safety_kd_(safety_kd),</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;          default_position_kp_(position_kp),</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;          default_position_kd_(position_kd)</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;    {</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;        pause_motors();</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;    }</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;</div><div class="line"><a name="l00124"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#ae8d7e4d37b3a0ba6966311c9813caec0">  124</a></span>&#160;    <span class="keyword">static</span> MotorBoards <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#ae8d7e4d37b3a0ba6966311c9813caec0">create_motor_boards</a>(</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;        <span class="keyword">const</span> std::array&lt;std::string, N_MOTOR_BOARDS&gt; &amp;can_ports)</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;    {</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;        <span class="comment">// setup can buses -----------------------------------------------------</span></div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;        std::array&lt;std::shared_ptr&lt;blmc_drivers::CanBus&gt;, N_MOTOR_BOARDS&gt;</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;            can_buses;</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; can_buses.size(); i++)</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;        {</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;            can_buses[i] = std::make_shared&lt;blmc_drivers::CanBus&gt;(can_ports[i]);</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;        }</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;        <span class="comment">// set up motor boards -------------------------------------------------</span></div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;        MotorBoards motor_boards;</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; motor_boards.size(); i++)</div><div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;        {</div><div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;            motor_boards[i] = std::make_shared&lt;blmc_drivers::CanBusMotorBoard&gt;(</div><div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;                can_buses[i], 1000, 10);</div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;        }</div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; motor_boards.size(); i++)</div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;        {</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;            motor_boards[i]-&gt;wait_until_ready();</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;        }</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;        <span class="keywordflow">return</span> motor_boards;</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;    }</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;    <span class="keywordtype">void</span> pause_motors()</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;    {</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; motor_boards_.size(); i++)</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;        {</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;            motor_boards_[i]-&gt;pause_motors();</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;        }</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;    }</div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;    Vector get_measured_index_angles()<span class="keyword"> const</span></div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;<span class="keyword">    </span>{</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;        <span class="keywordflow">return</span> joint_modules_.get_measured_index_angles();</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;    }</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;<span class="keyword">public</span>:</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;    Observation get_latest_observation()<span class="keyword"> override</span></div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;<span class="keyword">    </span>{</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;        Observation observation;</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;        observation.position = joint_modules_.get_measured_angles();</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;        observation.velocity = joint_modules_.get_measured_velocities();</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;        observation.torque = joint_modules_.get_measured_torques();</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;        <span class="keywordflow">return</span> observation;</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;    }</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;<span class="keyword">protected</span>:</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;    Action apply_action(<span class="keyword">const</span> Action &amp;desired_action)<span class="keyword"> override</span></div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;<span class="keyword">    </span>{</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;        <span class="keywordflow">if</span> (!is_initialized_)</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;        {</div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;            <span class="keywordflow">throw</span> std::runtime_error(</div><div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;                <span class="stringliteral">&quot;Robot needs to be initialized before applying actions.  Run &quot;</span></div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;                <span class="stringliteral">&quot;the `initialize()` method.&quot;</span>);</div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;        }</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;</div><div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;        <span class="keywordflow">return</span> apply_action_uninitialized(desired_action);</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;    }</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;    Action apply_action_uninitialized(<span class="keyword">const</span> Action &amp;desired_action)</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;    {</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;        <span class="keywordtype">double</span> start_time_sec = real_time_tools::Timer::get_current_time_sec();</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;        Observation observation = get_latest_observation();</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;        Action applied_action;</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;        applied_action.torque = desired_action.torque;</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;        applied_action.position = desired_action.position;</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;        <span class="comment">// Position controller</span></div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;        <span class="comment">// -------------------</span></div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;        <span class="comment">// TODO: add position limits</span></div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;        <span class="comment">//</span></div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;        <span class="comment">// NOTE: in the following lines, the Eigen function `unaryExpr` is</span></div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;        <span class="comment">// used to determine which elements of the vectors are NaN.  This is</span></div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;        <span class="comment">// because `isNaN()` was only added in Eigen 3.3 but we have to be</span></div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;        <span class="comment">// compatible with 3.2.</span></div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;        <span class="comment">// The std::isnan call needs to be wrapped in a lambda because it is</span></div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;        <span class="comment">// overloaded and unaryExpr cannot resolve which version to use when</span></div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;        <span class="comment">// passed directly.</span></div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;        <span class="comment">// Run the position controller only if a target position is set for at</span></div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;        <span class="comment">// least one joint.</span></div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;        <span class="keywordflow">if</span> (!applied_action.position</div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;                 .unaryExpr([](<span class="keywordtype">double</span> x) { return std::isnan(x); })</div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;                 .all())</div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;        {</div><div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;            <span class="comment">// Replace NaN-values with default gains</span></div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;            applied_action.position_kp =</div><div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;                applied_action.position_kp</div><div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;                    .unaryExpr([](<span class="keywordtype">double</span> x) { <span class="keywordflow">return</span> std::isnan(x); })</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;                    .select(default_position_kp_, desired_action.position_kp);</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;            applied_action.position_kd =</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;                applied_action.position_kd</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;                    .unaryExpr([](<span class="keywordtype">double</span> x) { <span class="keywordflow">return</span> std::isnan(x); })</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;                    .select(default_position_kd_, desired_action.position_kd);</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;</div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;            Vector position_error =</div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;                applied_action.position - observation.position;</div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;            <span class="comment">// simple PD controller</span></div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;            Vector position_control_torque =</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;                applied_action.position_kp.cwiseProduct(position_error) +</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;                applied_action.position_kd.cwiseProduct(observation.velocity);</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;            <span class="comment">// position_control_torque contains NaN for joints where target</span></div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;            <span class="comment">// position is set to NaN!  Filter those out and set the torque to</span></div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;            <span class="comment">// zero instead.</span></div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;            position_control_torque =</div><div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;                position_control_torque</div><div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;                    .unaryExpr([](<span class="keywordtype">double</span> x) { <span class="keywordflow">return</span> std::isnan(x); })</div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;                    .select(0, position_control_torque);</div><div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;</div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;            <span class="comment">// Add result of position controller to the torque command</span></div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;            applied_action.torque += position_control_torque;</div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;        }</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;        <span class="comment">// Safety Checks</span></div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;        <span class="comment">// -------------</span></div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;        <span class="comment">// limit to configured maximum torque</span></div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;        applied_action.torque =</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;            mct::clamp(applied_action.torque, -max_torque_Nm_, max_torque_Nm_);</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;        <span class="comment">// velocity damping to prevent too fast movements</span></div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;        applied_action.torque -= safety_kd_.cwiseProduct(observation.velocity);</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;        <span class="comment">// after applying checks, make sure we are still below the max. torque</span></div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;        applied_action.torque =</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;            mct::clamp(applied_action.torque, -max_torque_Nm_, max_torque_Nm_);</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;</div><div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;        joint_modules_.set_torques(applied_action.torque);</div><div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;        joint_modules_.send_torques();</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;</div><div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;        real_time_tools::Timer::sleep_until_sec(start_time_sec + 0.001);</div><div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;        <span class="keywordflow">return</span> applied_action;</div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;    }</div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;    <span class="keywordtype">void</span> shutdown()<span class="keyword"> override</span></div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;<span class="keyword">    </span>{</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;        pause_motors();</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;    }</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;</div><div class="line"><a name="l00297"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a56f4a67a3ae5ff481eebd969d1e6374c">  297</a></span>&#160;    <span class="keywordtype">bool</span> <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a56f4a67a3ae5ff481eebd969d1e6374c">home_on_index_after_negative_end_stop</a>(</div><div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;        <span class="keywordtype">double</span> torque_ratio, Vector home_offset_rad = Vector::Zero())</div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;    {</div><div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;        constexpr <span class="keywordtype">double</span> SEARCH_DISTANCE_LIMIT_RAD = 2.0;</div><div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;        <span class="comment">// TODO distance limit could be set based on gear ratio to be 1.5 motor</span></div><div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;        <span class="comment">// revolutions</span></div><div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;</div><div class="line"><a name="l00305"></a><span class="lineno">  305</span>&#160;        <span class="keywordflow">if</span> (has_endstop_)</div><div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;        {</div><div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;            constexpr uint32_t MIN_STEPS_MOVE_TO_END_STOP = 1000;</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;            constexpr uint32_t SIZE_VELOCITY_WINDOW = 100;</div><div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;            constexpr <span class="keywordtype">double</span> STOP_VELOCITY = 0.001;</div><div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;</div><div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;            static_assert(MIN_STEPS_MOVE_TO_END_STOP &gt; SIZE_VELOCITY_WINDOW,</div><div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;                          <span class="stringliteral">&quot;MIN_STEPS_MOVE_TO_END_STOP has to be bigger than&quot;</span></div><div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;                          <span class="stringliteral">&quot; SIZE_VELOCITY_WINDOW to ensure correct computation&quot;</span></div><div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;                          <span class="stringliteral">&quot; of average velocity.&quot;</span>);</div><div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;</div><div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;            <span class="comment">// Move until velocity drops to almost zero (= joints hit the end</span></div><div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;            <span class="comment">// stops) but at least for MIN_STEPS_MOVE_TO_END_STOP time steps.</span></div><div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;            <span class="comment">// TODO: add timeout to this loop?</span></div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;            std::vector&lt;Vector&gt; running_velocities(SIZE_VELOCITY_WINDOW);</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;            Vector summed_velocities = Vector::Zero();</div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;            uint32_t step_count = 0;</div><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;            <span class="keywordflow">while</span> (step_count &lt; MIN_STEPS_MOVE_TO_END_STOP ||</div><div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;                   (summed_velocities.maxCoeff() / SIZE_VELOCITY_WINDOW &gt;</div><div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;                    STOP_VELOCITY))</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;            {</div><div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;                Vector torques = -1 * torque_ratio * get_max_torques();</div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;                apply_action_uninitialized(torques);</div><div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;                Vector abs_velocities =</div><div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;                    get_latest_observation().velocity.cwiseAbs();</div><div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;</div><div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;                uint32_t running_index = step_count % SIZE_VELOCITY_WINDOW;</div><div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;                <span class="keywordflow">if</span> (step_count &gt;= SIZE_VELOCITY_WINDOW)</div><div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;                {</div><div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;                    summed_velocities -= running_velocities[running_index];</div><div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;                }</div><div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;                running_velocities[running_index] = abs_velocities;</div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;                summed_velocities += abs_velocities;</div><div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;                step_count++;</div><div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;            }</div><div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;        }</div><div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;</div><div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;        <span class="comment">// Home on encoder index</span></div><div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;        <a class="code" href="namespaceblmc__robots.html#aa1075809042ff261e4b0a20d161448b6">HomingReturnCode</a> homing_status = joint_modules_.execute_homing(</div><div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;            SEARCH_DISTANCE_LIMIT_RAD, home_offset_rad);</div><div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;</div><div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;        rt_printf(<span class="stringliteral">&quot;Finished homing.\n&quot;</span>);</div><div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;</div><div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;        <span class="keywordflow">return</span> homing_status == <a class="code" href="namespaceblmc__robots.html#aa1075809042ff261e4b0a20d161448b6afd9f6519d3f45276829fe74eb2c4bb85">HomingReturnCode::SUCCEEDED</a>;</div><div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;    }</div><div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;</div><div class="line"><a name="l00365"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a1defedb3825ccac7cd8511d209bcae61">  365</a></span>&#160;    <span class="keywordtype">bool</span> <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a1defedb3825ccac7cd8511d209bcae61">move_to_position</a>(<span class="keyword">const</span> Vector &amp;goal_pos,</div><div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;                          <span class="keyword">const</span> <span class="keywordtype">double</span> tolerance,</div><div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;                          <span class="keyword">const</span> uint32_t timeout_cycles)</div><div class="line"><a name="l00368"></a><span class="lineno">  368</span>&#160;    {</div><div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;        <span class="keywordtype">bool</span> reached_goal = <span class="keyword">false</span>;</div><div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;        uint32_t cycle_count = 0;</div><div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;</div><div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;        <span class="keywordflow">while</span> (!reached_goal &amp;&amp; cycle_count &lt; timeout_cycles)</div><div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;        {</div><div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;            apply_action(Action::Position(goal_pos));</div><div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;</div><div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;            <span class="keyword">const</span> Vector position_error =</div><div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;                goal_pos - get_latest_observation().position;</div><div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;            <span class="keyword">const</span> Vector velocity = get_latest_observation().velocity;</div><div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;</div><div class="line"><a name="l00380"></a><span class="lineno">  380</span>&#160;            <span class="comment">// Check if the goal is reached (position error below tolerance and</span></div><div class="line"><a name="l00381"></a><span class="lineno">  381</span>&#160;            <span class="comment">// velocity close to zero).</span></div><div class="line"><a name="l00382"></a><span class="lineno">  382</span>&#160;            constexpr <span class="keywordtype">double</span> ZERO_VELOCITY = 1e-4;</div><div class="line"><a name="l00383"></a><span class="lineno">  383</span>&#160;            reached_goal = ((position_error.array().abs() &lt; tolerance).all() &amp;&amp;</div><div class="line"><a name="l00384"></a><span class="lineno">  384</span>&#160;                            (velocity.array().abs() &lt; ZERO_VELOCITY).all());</div><div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;</div><div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;            cycle_count++;</div><div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;        }</div><div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;</div><div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160;        <span class="keywordflow">return</span> reached_goal;</div><div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;    }</div><div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;</div><div class="line"><a name="l00400"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a41b2ff96e0a687b0e1ae06ce1502d4db">  400</a></span>&#160;    <span class="keywordtype">void</span> <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a41b2ff96e0a687b0e1ae06ce1502d4db">initialize</a>()<span class="keyword"> override</span></div><div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;<span class="keyword">    </span>{</div><div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;        joint_modules_.set_position_control_gains(default_position_kp_,</div><div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160;                                                  default_position_kd_);</div><div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;</div><div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;        is_initialized_ = home_on_index_after_negative_end_stop(</div><div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;            calibration_parameters_.torque_ratio, home_offset_rad_);</div><div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;</div><div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;        <span class="keywordflow">if</span> (is_initialized_)</div><div class="line"><a name="l00409"></a><span class="lineno">  409</span>&#160;        {</div><div class="line"><a name="l00410"></a><span class="lineno">  410</span>&#160;            <span class="keywordtype">bool</span> reached_goal =</div><div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;                move_to_position(initial_position_rad_,</div><div class="line"><a name="l00412"></a><span class="lineno">  412</span>&#160;                                 calibration_parameters_.position_tolerance_rad,</div><div class="line"><a name="l00413"></a><span class="lineno">  413</span>&#160;                                 calibration_parameters_.move_timeout);</div><div class="line"><a name="l00414"></a><span class="lineno">  414</span>&#160;            <span class="keywordflow">if</span> (!reached_goal)</div><div class="line"><a name="l00415"></a><span class="lineno">  415</span>&#160;            {</div><div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;                rt_printf(<span class="stringliteral">&quot;Failed to reach goal, timeout exceeded.\n&quot;</span>);</div><div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;            }</div><div class="line"><a name="l00418"></a><span class="lineno">  418</span>&#160;        }</div><div class="line"><a name="l00419"></a><span class="lineno">  419</span>&#160;</div><div class="line"><a name="l00420"></a><span class="lineno">  420</span>&#160;        pause_motors();</div><div class="line"><a name="l00421"></a><span class="lineno">  421</span>&#160;    }</div><div class="line"><a name="l00422"></a><span class="lineno">  422</span>&#160;</div><div class="line"><a name="l00423"></a><span class="lineno">  423</span>&#160;<span class="keyword">protected</span>:</div><div class="line"><a name="l00424"></a><span class="lineno">  424</span>&#160;</div><div class="line"><a name="l00425"></a><span class="lineno">  425</span>&#160;    <a class="code" href="classblmc__robots_1_1BlmcJointModules.html">BlmcJointModules&lt;N_JOINTS&gt;</a> joint_modules_;</div><div class="line"><a name="l00426"></a><span class="lineno">  426</span>&#160;    MotorBoards motor_boards_;</div><div class="line"><a name="l00427"></a><span class="lineno">  427</span>&#160;</div><div class="line"><a name="l00428"></a><span class="lineno">  428</span>&#160;    <span class="comment">// TODO: this should probably go away</span></div><div class="line"><a name="l00429"></a><span class="lineno">  429</span>&#160;    <span class="keywordtype">double</span> max_torque_Nm_;</div><div class="line"><a name="l00430"></a><span class="lineno">  430</span>&#160;</div><div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;    <a class="code" href="structblmc__robots_1_1CalibrationParameters.html">CalibrationParameters</a> calibration_parameters_;</div><div class="line"><a name="l00432"></a><span class="lineno">  432</span>&#160;</div><div class="line"><a name="l00434"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a1c884b0a6a398dc17c9f642349637143">  434</a></span>&#160;    Vector <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a1c884b0a6a398dc17c9f642349637143">safety_kd_</a>;</div><div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;</div><div class="line"><a name="l00442"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a4397347cbb3ad2e75abc4ad1613b119e">  442</a></span>&#160;    Vector home_offset_rad_ = Vector::Zero();</div><div class="line"><a name="l00443"></a><span class="lineno">  443</span>&#160;</div><div class="line"><a name="l00445"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a5f17c2843ed4cb09953338143e91b45f">  445</a></span>&#160;    Vector initial_position_rad_ = Vector::Zero();</div><div class="line"><a name="l00446"></a><span class="lineno">  446</span>&#160;</div><div class="line"><a name="l00448"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#ae3cc54a0b1bad799b80ab72641f9d24e">  448</a></span>&#160;    Vector <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#ae3cc54a0b1bad799b80ab72641f9d24e">default_position_kp_</a>;</div><div class="line"><a name="l00450"></a><span class="lineno"><a class="line" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a466bd0a736e829536b5d8b6203b68ed9">  450</a></span>&#160;    Vector <a class="code" href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a466bd0a736e829536b5d8b6203b68ed9">default_position_kd_</a>;</div><div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;</div><div class="line"><a name="l00452"></a><span class="lineno">  452</span>&#160;    <span class="keywordtype">bool</span> is_initialized_ = <span class="keyword">false</span>;</div><div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;</div><div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160;<span class="keyword">public</span>:</div><div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;    Vector get_max_torques()<span class="keyword"> const</span></div><div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;<span class="keyword">    </span>{</div><div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;        <span class="keywordflow">return</span> max_torque_Nm_ * Vector::Ones();</div><div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;    }</div><div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;};</div><div class="line"><a name="l00460"></a><span class="lineno">  460</span>&#160;</div><div class="line"><a name="l00461"></a><span class="lineno">  461</span>&#160;}  <span class="comment">// namespace blmc_robots</span></div><div class="ttc" id="structblmc__robots_1_1MotorParameters_html"><div class="ttname"><a href="structblmc__robots_1_1MotorParameters.html">blmc_robots::MotorParameters</a></div><div class="ttdoc">Parameters related to the motor. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:26</div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html_a4da9d51841e9b2995e597ae52c460586"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a4da9d51841e9b2995e597ae52c460586">blmc_robots::NJointBlmcRobotDriver::has_endstop_</a></div><div class="ttdeci">const bool has_endstop_</div><div class="ttdoc">True if the joints have mechanical end stops, false if not. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:95</div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html_a466bd0a736e829536b5d8b6203b68ed9"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a466bd0a736e829536b5d8b6203b68ed9">blmc_robots::NJointBlmcRobotDriver::default_position_kd_</a></div><div class="ttdeci">Vector default_position_kd_</div><div class="ttdoc">default D-gain for position controller. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:450</div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html_a1c884b0a6a398dc17c9f642349637143"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a1c884b0a6a398dc17c9f642349637143">blmc_robots::NJointBlmcRobotDriver::safety_kd_</a></div><div class="ttdeci">Vector safety_kd_</div><div class="ttdoc">D-gain to dampen velocity. Set to zero to disable damping. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:434</div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html">blmc_robots::NJointBlmcRobotDriver</a></div><div class="ttdoc">Base class for simple n-joint BLMC robots. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:66</div></div>
<div class="ttc" id="classblmc__robots_1_1BlmcJointModules_html"><div class="ttname"><a href="classblmc__robots_1_1BlmcJointModules.html">blmc_robots::BlmcJointModules&lt; N_JOINTS &gt;</a></div></div>
<div class="ttc" id="structblmc__robots_1_1MotorParameters_html_a6a8158b9ac1633bab6181043d04ab655"><div class="ttname"><a href="structblmc__robots_1_1MotorParameters.html#a6a8158b9ac1633bab6181043d04ab655">blmc_robots::MotorParameters::gear_ratio</a></div><div class="ttdeci">double gear_ratio</div><div class="ttdoc">Gear ratio between motor and joint. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:40</div></div>
<div class="ttc" id="blmc__joint__module_8hpp_html"><div class="ttname"><a href="blmc__joint__module_8hpp.html">blmc_joint_module.hpp</a></div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html_a41b2ff96e0a687b0e1ae06ce1502d4db"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a41b2ff96e0a687b0e1ae06ce1502d4db">blmc_robots::NJointBlmcRobotDriver::initialize</a></div><div class="ttdeci">void initialize() override</div><div class="ttdoc">Find home position of all joints and move to start position. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:400</div></div>
<div class="ttc" id="namespaceblmc__robots_html_aa1075809042ff261e4b0a20d161448b6afd9f6519d3f45276829fe74eb2c4bb85"><div class="ttname"><a href="namespaceblmc__robots.html#aa1075809042ff261e4b0a20d161448b6afd9f6519d3f45276829fe74eb2c4bb85">blmc_robots::HomingReturnCode::SUCCEEDED</a></div><div class="ttdoc">Homing is succeeded. </div></div>
<div class="ttc" id="structblmc__robots_1_1MotorParameters_html_ae8870c744aae11567280006ea7b7ed25"><div class="ttname"><a href="structblmc__robots_1_1MotorParameters.html#ae8870c744aae11567280006ea7b7ed25">blmc_robots::MotorParameters::max_current_A</a></div><div class="ttdeci">double max_current_A</div><div class="ttdoc">Maximum current that can be sent to the motor [A]. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:29</div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html_a1defedb3825ccac7cd8511d209bcae61"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a1defedb3825ccac7cd8511d209bcae61">blmc_robots::NJointBlmcRobotDriver::move_to_position</a></div><div class="ttdeci">bool move_to_position(const Vector &amp;goal_pos, const double tolerance, const uint32_t timeout_cycles)</div><div class="ttdoc">Move to given goal position using PD control. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:365</div></div>
<div class="ttc" id="structblmc__robots_1_1MotorParameters_html_a960e7f64dae8cd08837f8bff620eacfb"><div class="ttname"><a href="structblmc__robots_1_1MotorParameters.html#a960e7f64dae8cd08837f8bff620eacfb">blmc_robots::MotorParameters::torque_constant_NmpA</a></div><div class="ttdeci">double torque_constant_NmpA</div><div class="ttdoc">Torque constant K_t of the motor [Nm/A]. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:32</div></div>
<div class="ttc" id="structblmc__robots_1_1CalibrationParameters_html_a1075d6b60013e060c22748a672bf08cb"><div class="ttname"><a href="structblmc__robots_1_1CalibrationParameters.html#a1075d6b60013e060c22748a672bf08cb">blmc_robots::CalibrationParameters::move_timeout</a></div><div class="ttdeci">double move_timeout</div><div class="ttdoc">Timeout for reaching the starting position. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:53</div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html_a56f4a67a3ae5ff481eebd969d1e6374c"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html#a56f4a67a3ae5ff481eebd969d1e6374c">blmc_robots::NJointBlmcRobotDriver::home_on_index_after_negative_end_stop</a></div><div class="ttdeci">bool home_on_index_after_negative_end_stop(double torque_ratio, Vector home_offset_rad=Vector::Zero())</div><div class="ttdoc">Homing on negative end stop and encoder index. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:297</div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html_ae3cc54a0b1bad799b80ab72641f9d24e"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html#ae3cc54a0b1bad799b80ab72641f9d24e">blmc_robots::NJointBlmcRobotDriver::default_position_kp_</a></div><div class="ttdeci">Vector default_position_kp_</div><div class="ttdoc">default P-gain for position controller. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:448</div></div>
<div class="ttc" id="common__header_8hpp_html"><div class="ttname"><a href="common__header_8hpp.html">common_header.hpp</a></div><div class="ttdoc">The hardware wrapper of the quadruped. </div></div>
<div class="ttc" id="classblmc__robots_1_1NJointBlmcRobotDriver_html_ae8d7e4d37b3a0ba6966311c9813caec0"><div class="ttname"><a href="classblmc__robots_1_1NJointBlmcRobotDriver.html#ae8d7e4d37b3a0ba6966311c9813caec0">blmc_robots::NJointBlmcRobotDriver::create_motor_boards</a></div><div class="ttdeci">static MotorBoards create_motor_boards(const std::array&lt; std::string, N_MOTOR_BOARDS &gt; &amp;can_ports)</div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:124</div></div>
<div class="ttc" id="namespacerobot__interfaces_html"><div class="ttname"><a href="namespacerobot__interfaces.html">robot_interfaces</a></div></div>
<div class="ttc" id="structblmc__robots_1_1CalibrationParameters_html"><div class="ttname"><a href="structblmc__robots_1_1CalibrationParameters.html">blmc_robots::CalibrationParameters</a></div><div class="ttdoc">Parameters for the joint calibration (homing and go to initial pose). </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:46</div></div>
<div class="ttc" id="namespaceblmc__robots_html_aa1075809042ff261e4b0a20d161448b6"><div class="ttname"><a href="namespaceblmc__robots.html#aa1075809042ff261e4b0a20d161448b6">blmc_robots::HomingReturnCode</a></div><div class="ttdeci">HomingReturnCode</div><div class="ttdoc">Possible return values of the homing. </div><div class="ttdef"><b>Definition:</b> blmc_joint_module.hpp:28</div></div>
<div class="ttc" id="structblmc__robots_1_1CalibrationParameters_html_a84662e07c2b77e035e28092584ea0ab3"><div class="ttname"><a href="structblmc__robots_1_1CalibrationParameters.html#a84662e07c2b77e035e28092584ea0ab3">blmc_robots::CalibrationParameters::position_tolerance_rad</a></div><div class="ttdeci">double position_tolerance_rad</div><div class="ttdoc">Tolerance for reaching the starting position. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:51</div></div>
<div class="ttc" id="namespaceblmc__robots_html"><div class="ttname"><a href="namespaceblmc__robots.html">blmc_robots</a></div><div class="ttdef"><b>Definition:</b> blmc_joint_module.cpp:15</div></div>
<div class="ttc" id="structblmc__robots_1_1CalibrationParameters_html_a85b403cbe9a0720b3b6ed46187e31fa6"><div class="ttname"><a href="structblmc__robots_1_1CalibrationParameters.html#a85b403cbe9a0720b3b6ed46187e31fa6">blmc_robots::CalibrationParameters::torque_ratio</a></div><div class="ttdeci">double torque_ratio</div><div class="ttdoc">Ratio of the max. torque that is used to find the end stop. </div><div class="ttdef"><b>Definition:</b> n_joint_blmc_robot_driver.hpp:49</div></div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="navelem"><a class="el" href="dir_68267d1309a1af8e8297ef4c3efbcdba.html">src</a></li><li class="navelem"><a class="el" href="dir_80e6cdd9bbc895bd262b4f65e652822c.html">catkin</a></li><li class="navelem"><a class="el" href="dir_b7f9efe3285d4a585ae30572be0808b1.html">robots</a></li><li class="navelem"><a class="el" href="dir_ee86b8bcb6635cb36ad14bd888f09517.html">blmc_robots</a></li><li class="navelem"><a class="el" href="dir_ff3606c6e0aa43abff10c4365bd5a19c.html">include</a></li><li class="navelem"><a class="el" href="dir_c66acd4681b95343b3d51cde3fc288b6.html">blmc_robots</a></li><li class="navelem"><a class="el" href="n__joint__blmc__robot__driver_8hpp.html">n_joint_blmc_robot_driver.hpp</a></li>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
